package scu.maqiang.numeric;

import java.util.Arrays;

/**
 * ���ܾ�����������
 * @author ��ǿ
 *
 */
public class IterDSolver extends Solver{
	
	public IterDSolver() {
		
	}
	
	public static void main(String[] args) {
		double[][] A = {{ 3, -1, 0, 0, 0, 0.5}, 
	     {-1, 3, -1, 0, 0.5, 0},
	     {0, -1, 3, -1, 0, 0}, 
	     {0, 0, -1, 3, -1, 0},
	     {0, 0.5, 0, -1, 3, -1},
	     {0.5, 0, 0, 0, -1, 3}};
		double[] b = {2.5, 1.5, 1, 1, 1.5, 2.5};
		double[] x = new double[6];
		IterDSolver solver = new IterDSolver();
		solver.Jacobi(A, b, x);
		System.out.println(MVO.toString(x));
	}
	
	
	public IterDSolver(double eps, int maxIteration) {
		this.eps = eps;
		this.maxIteration = maxIteration;
	}
	
	/**
	 * Jacobi迭代算法
	 * @param A ��ά����
	 * @param b �Ҷ���
	 * @param x0 ������ʼֵ, ͬʱ���������
	 * @return ��������, ��������������������������-1
	 */
	public int Jacobi(double[][] A, double[] b, double[] x0) {
		int n = A[0].length;
		double[] x1 = x0.clone();
		double err = eps + 1.0;
		double a00 = 0.0;
		int k;
		for (k = 0; k < maxIteration; k++) {
			if(err < Constants.Er) {
				break;
			}
			for(int i = 0; i < n; i++) {
				double temp = 0.0;
				for(int j = 0; j < n; j++) {
					if (i != j) {
						temp += A[i][j] * x0[j];
					} else {
						a00 = A[i][j];
					}
				}
				x1[i] = (b[i] - temp) / a00;
			}
			err = MVO.distance(x0, x1);
			System.arraycopy(x1, 0, x0, 0, n);
			System.out.println(Arrays.toString(x1));
		}
		if(k == maxIteration) {
			System.out.println("Iteration Has Reached Maximum Time! ");
			return -1;
		}
		System.out.println("Iteration Time " + k);
		return k;
	}
	
	/**
	 * ʹ��Gram-Schmidt��������������A������ȫQR�ֽ�
	 * @param A mxn�;���
	 * @param Q mxn�;���
	 * @param R nxn�;���
	 */
	public static void QR_GS(double[][] A, double[][] Q, double[][] R) {
		int m = A.length;
		int n = A[0].length;
		MVO.fill(Q, 0.0);
		MVO.fill(R, 0.0);
		for (int j = 0; j < n; j++) {
			double[] y = MVO.col(A, j);
			for(int i = 0; i < j; i++) {
				double[] q_i = MVO.col(Q, i);
				R[i][j] = MVO.dot_product(q_i, y);
				for(int k = 0; k < m; k++) {
					y[k] -= R[i][j] * q_i[k];
				}
			}
			R[j][j] = MVO.L2Norm(y);
			for(int k = 0; k < m; k++) {
				Q[k][j] = y[k] / R[j][j];
			}
		}
	}
	
	
	/**
	 * ʹ��Gram-Schmidt��������������A����ȫQR�ֽ�
	 * @param A mxn�;���
	 * @param Q mxm����������
	 * @param R mxn�;���
	 */
	public void QR_GSC(double[][] A, double[][] Q, double[][] R) {
		//Ҫ��m >= n
		int m = A.length;
		int n = A[0].length;
		double[][] AA = new double[m][];
		for(int i = 0; i < m; i++) {
			AA[i] = new double[m];
			for(int j = 0; j < n; j++) {
				AA[i][j] = A[i][j];
			}
		}
		if (m > n) {
			int idx = 0;
			for(int j = n; j < m; j++) {
				AA[idx++][j] = 1.0;
			}
		}
		//System.out.println(MVO.toString(AA));
		MVO.fill(Q, 0.0);
		MVO.fill(R, 0.0);
		for (int j = 0; j < m; j++) {
			double[] y = MVO.col(AA, j);
			for(int i = 0; i < j; i++) {
				double[] q_i = MVO.col(Q, i);
				double rij = MVO.dot_product(q_i, y);
				//R[i][j] = MVO.dot_product(q_i, y);
				for(int k = 0; k < m; k++) {
					y[k] -= rij * q_i[k];
				}
				if (j < n) {
					R[i][j] = rij;
				}
			}
			double rjj = MVO.L2Norm(y);
			//R[j][j] = MVO.L2Norm(y);
			for(int k = 0; k < m; k++) {
				Q[k][j] = y[k] / rjj;
			}
			if (j < n) {
				R[j][j] = rjj;
			}
		}
		System.out.println(MVO.toString(Q));
		System.out.println(MVO.toString(R));
	}
	
	/**
	 * ʹ��QR�ֽ�����С�������⣬����Ϊ��Hessenberg����ʹ��Givens�任����QR�ֽ� 
	 * @param A (n+1)xn����Hessenberg��
	 * @param b n+1ά����
	 * @param x nά���飬������С���˽�
	 * @return ���ض��������
	 */
	public static double MLS_QR_Givens(double[][] A, double[] b, double[] x) {
		int n = A.length;
		double c, s, t1, t2;
		double[] csEta = new double[3];
		for(int i = 0; i < n - 1; i++) {
			givens(A[i][i], A[i+1][i], csEta);
			A[i][i] = csEta[2];
			A[i+1][i] = 0.0;
			c = csEta[0];
			s = csEta[1];
			for(int j = i + 1; j < n - 1; j++) {
				t1 = A[i][j];
				t2 = A[i+1][j];
				A[i][j] = t1 * c + t2 * s;
				A[i+1][j] = t2 * c - t1 * s;
			}
			t1 = b[i];
			t2 = b[i + 1];
			b[i] = t1 * c + t2 * s;
			b[i+1] = t2 * c - t1 * s;
		}
		
		n = A[0].length;
		x[n - 1] = b[n - 1] / A[n - 1][n - 1];
		for(int i = n - 2; i >=0; i--) {
			double temp = 0.0;
			for(int j = i + 1; j < n; j++) {
				temp += A[i][j] * x[j];
			}
			x[i] = (b[i] - temp)/A[i][i];
		}
		
		//System.out.println(Arrays.toString(x));
		//System.out.println(MVO.L2Norm(MVO.add(b, -1.0, MVO.matmul(A, x))));
		//System.exit(0);
		return MVO.L2Norm(MVO.add(b, -1.0, MVO.matmul(A, x)));
	}
	
	
	public static double MLS_QR_Givens2(double[][] A, int m1, int m2, int n1, int n2, double[] b, double[] x) {
		//int n = A.length;
		int m = m2 - m1 + 1;
		int n = m2 - m1 + 1;
		//int n = col1 - col0 + 1;
		double c, s, t1, t2;
		double[] csEta = new double[3];
		for(int i = 0; i < n - 1; i++) {
			givens(A[i][i], A[i+1][i], csEta);
			A[i][i] = csEta[2];
			A[i+1][i] = 0.0;
			c = csEta[0];
			s = csEta[1];
			for(int j = i + 1; j < n - 1; j++) {
				t1 = A[i][j];
				t2 = A[i+1][j];
				A[i][j] = t1 * c + t2 * s;
				A[i+1][j] = t2 * c - t1 * s;
			}
			t1 = b[i];
			t2 = b[i + 1];
			b[i] = t1 * c + t2 * s;
			b[i+1] = t2 * c - t1 * s;
		}
		
		n = n2 - n1 + 1;
		//n = A[0].length;
		x[n - 1] = b[n - 1] / A[n - 1][n - 1];
		for(int i = n - 2; i >=0; i--) {
			double temp = 0.0;
			for(int j = i + 1; j < n; j++) {
				temp += A[i][j] * x[j];
			}
			x[i] = (b[i] - temp)/A[i][i];
		}
//		System.out.println(Arrays.toString(x));
//		System.exit(0);
		double L2Error = 0.0;
		for(int i = 0; i < m; i++) {
			double temp = 0.0;
			for(int j = 0; j < n; j++) {
				temp += A[i][j] * x[j];
			}
			L2Error += (b[i] - temp) * (b[i] - temp);
		}
		//System.out.println(L2Error);
		//System.exit(0);
		return Math.sqrt(L2Error);
		
		//MVO.L2Norm(MVO.add(b, -1.0, MVO.matmul(A, x)));
	}
	
	public static void givens(double a, double b, double[] csEta) {
		double c, s, t;
		    if (b == 0) {
		    	csEta[0] = 1.0;
		    	csEta[1] = 0.0;
		    	csEta[2] = a;
		    }
		    if (a == 0) {
		    	csEta[0] = 0.0;
		    	csEta[1] = 1.0;
		    	csEta[2] = b;
		    }
		    
		    if (Math.abs(b) >= Math.abs(a))  {
		    	t = a / b;
				s = 1.0 / Math.sqrt(1.0 + t*t);
				c = s * t;
				csEta[0] = c;
				csEta[1] = s;
				csEta[2] = Math.abs(b) / s;
		    }else {
		    	t = b / a;
		    	c = 1.0 / Math.sqrt(1.0 + t*t);
		    	s = c * t;				
		    	csEta[0] = c;
				csEta[1] = s;
				csEta[2] = Math.abs(a)/ c;
		    }
	}
	
	
	
	/**
	 * ʹ��QR�ֽ�����С��������, ����QR�ֽ�ʹ��Householder�任���
	 * @param A mxn�;���Ҫ��m>n, 
	 * @param b mά���飬 ��ʾ�Ҷ���
	 * @param x nά���飬��С��������Ax=b�Ľ�
	 * @return ��С�������
	 */
	public static double MLS_QR_HH(double[][] A, double[] b, double[] x) {
		int m = A.length;
		int n = A[0].length;
		double[][] Q = new double[m][m];
		double[][] R = new double[m][n];
		//ʹ��HouseHolder�任��ɾ����QR�ֽ�
		MVO.QR_HH(A, Q, R);
		//System.out.println(MVO.toString(Q));
		//System.out.println(MVO.toString(R));
		//QT m x m�;���
		double[][] QT = MVO.transpose(Q);
		//System.out.println(MVO.toString(MVO.matmul(QT, Q)));
		//d mά����
		double[] d = MVO.matmul(QT, b);
		//System.out.println(Arrays.toString(d));
		double error = 0.0;
		//�ش������С���˵Ľ�
		//System.out.println(Arrays.toString(d));
		x[n - 1] = d[n - 1] / R[n - 1][n - 1];
		for(int i = n - 2; i >=0; i--) {
			double temp = 0.0;
			for(int j = i + 1; j < n; j++) {
				temp += R[i][j] * x[j];
			}
			x[i] = (d[i] - temp)/R[i][i];
		}
		
		//������С�������
		for(int i = n; i < m; i++) {
			error += d[i] * d[i];
		}
		error = Math.sqrt(error);
		return error;
	}
	
	public static double MLS_QR_GS(double[][] A, double[] b, double[] x) {
		int m = A.length;
		int n = A[0].length;
		double[][] Q = new double[m][n];
		double[][] R = new double[n][n];
		//ʹ��HouseHolder�任��ɾ����QR�ֽ�
		QR_GS(A, Q, R);
		//System.out.println(MVO.toString(Q));
		//System.out.println(MVO.toString(R));
		//QT m x m�;���
		double[][] QT = MVO.transpose(Q);
		//System.out.println(MVO.toString(MVO.matmul(QT, Q)));
		//d mά����
		double[] d = MVO.matmul(QT, b);
		//System.out.println(Arrays.toString(d));
		//�ش������С���˵Ľ�
		//System.out.println(Arrays.toString(d));
		x[n - 1] = d[n - 1] / R[n - 1][n - 1];
		for(int i = n - 2; i >=0; i--) {
			double temp = 0.0;
			for(int j = i + 1; j < n; j++) {
				temp += R[i][j] * x[j];
			}
			x[i] = (d[i] - temp)/R[i][i];
		}
		return MVO.L2Norm(MVO.add(b, -1.0, MVO.matmul(A, x)));
	}
	
	public void GMRES(double[][] A, double[] b, double[] x, int m) {
		int n = A.length;
		ParamCheck.checkEqual(n, A[0].length);
		ParamCheck.checkEqual(n, b.length);
		ParamCheck.checkEqual(n, x.length);
		double[] r = MVO.add(b, -1.0, MVO.matmul(A, x));
		double[][] H = new double[m+1][m];
		double[][] Q = new double[n][m+1];
		double[] x0 = x.clone();
		double[] f = new double[m+1];
		f[0] = MVO.L2Norm(r);
		for(int i = 0; i < n; i++) {
			Q[i][0] = r[i] / f[0];
		}
		for(int k = 0; k < m; k++) {
			double[] colK = MVO.col(Q, k);
			double[] y = MVO.matmul(A, colK);
			for(int j = 0; j <= k; j++) {
				double[] colJ = MVO.col(Q, j);
				H[j][k] = MVO.dot_product(colJ, y);
				for(int t = 0; t < n; t++) {
					y[t] -= H[j][k] * colJ[t];
				}
			}
			H[k+1][k] = MVO.L2Norm(y);
			for(int t = 0; t < n; t++) {
				Q[t][k+1] = y[t] / H[k+1][k];
			}
			
			double[] c = new double[k+1];
			double error = MLS_QR_HH(MVO.subMatrix(H, 0, k+1, 0, k), MVO.subVector(f, 0, k+1), c);
			MVO.add(x0, 1.0, MVO.matmul(MVO.subMatrix(Q, 0, n-1, 0, k), c), x);
			//System.out.println("Error = " + error);
			if (Math.abs(error) < Constants.Er) {
				break;
			}
		}
	}
	
	public int CGNE(double[][] A, double[] b, double[] x0, int verbosity) {
		int n = b.length;
		long begin = System.nanoTime();
		double[] r0 = MVO.add(b, -1.0, MVO.matmul(A, x0));
		double[] p0 = MVO.matmulT(A, r0);
		double[] Ap0 = new double[n];
		double temp1, temp2, alpha, beta, r2;
		r2 = MVO.L2Norm(r0);
		int i;
		for (i = 0; i < maxIteration; i++) {
			if (r2 < eps) {
				break;
			}
			temp1 = 0.0;
			temp2 = 0.0;
//			matU.mul(p0, Ap0);
			MVO.matmul(A, p0, Ap0);
			for (int j = 0; j < n; j++) {
				temp1 += r0[j] * r0[j];
				temp2 += p0[j] * p0[j];
			}
			alpha = temp1 / temp2 ; 
			temp2 = temp1;
			temp1 = 0.0;
			for (int j = 0; j < n; j++) {
				x0[j] += alpha * p0[j];
				r0[j] -= alpha * Ap0[j];
				temp1 += r0[j] * r0[j];
			}
			beta = temp1 / temp2;
			//matU.mulT(r0, Ap0);
			MVO.matmulT(A, r0, Ap0);
			for (int j = 0; j < n; j++) {
				p0[j] = Ap0[j] + beta * p0[j];
			}
			r2 = MVO.L2Norm(r0);
			if (verbosity >= 2) {
				System.out.println();
				System.out.println("Iteration: " + (i + 1));
				System.out.printf("min: %20.10f\t max: %20.10f\n", MVO.min(x0), MVO.max(x0));
				System.out.printf("Error: %20.10f\n", r2);
				System.out.println();
			}
		}
		if (verbosity >= 1) {
			System.out.println();
			System.out.println("Total iteration: " + i);
			System.out.printf("min: %20.10f\t max: %20.10f\n", MVO.min(x0), MVO.max(x0));
			long end = System.nanoTime();
			System.out.println("Solve time: " + (end - begin) / 1.0e9 + " s");
			System.out.println();
		}
		if (i == maxIteration) {
			throw new IllegalArgumentException("Iteration Has Reached Maximum Time");
		}
		return i;
	}

	public int CGS(double[][] A, double[] b, double[] x0) {
		int n = A.length;
		double[] r0 = MVO.add(b, -1.0, MVO.matmul(A, x0));
		double[] u0 = r0.clone();
		double[] z0 = MVO.add(r0, 1.0);
		double[] p0 = r0.clone();
		double[] q0 = new double[n];
		double[] v0 = MVO.matmul(A, p0);
		//double alpha = 0.0;
		//double beta = 0.0;
		int k;
		for (k = 0; k < maxIteration; k++) {
			if (MVO.L2Norm(r0) < eps) {
				break;
			}
			
			double temp1 = 0.0;
			double temp2 = 0.0;
			for(int j = 0; j < n; j++) {
				temp1 += r0[j] * z0[j];
				temp2 += v0[j] * z0[j];
			}
			double alpha = temp1 / temp2;
//			for(int i = 0; i < n; i++) {
//				q0[i] = u0[i] - alpha * v0[i];
//			}
//			double[] uq = MVO.add(u0, q0);
//			double[] Auq = A.mul(uq);
//			
//			temp2 = temp1;
//			temp1 = 0.0;
//			for(int i = 0; i < n; i++) {
//				x0[i] += alpha * uq[i];
//				r0[i] -= alpha * Auq[i];
//				temp1 += r0[i] * z0[i];
//			}
			for(int i = 0; i < n; i++) {
				q0[i] = u0[i] - alpha * v0[i];
				v0[i] = u0[i] + q0[i];

			}
			
//			System.out.println("v0 = " + MVO.toString(v0));
			//double[] uq = MVO.add(u0, q0);
			//double[] Auq = A.mul(v0);
			temp2 = temp1;
			temp1 = 0.0;
			//matU.mul(v0, u0);
			MVO.matmul(A, v0, u0);
			for(int i = 0; i < n; i++) {
				x0[i] += alpha * v0[i];
				r0[i] -= alpha * u0[i];
				temp1 += r0[i] * z0[i];
			}
			double beta = temp1 / temp2;
			for(int i = 0; i < n; i++) {
				u0[i] = r0[i] + beta * q0[i];
				p0[i] = u0[i] + beta *(q0[i] + beta * p0[i]);

			}
			//matU.mul(p0, v0);
			MVO.matmul(A, p0, v0);
			//v0 = A.mul(p0);
//			System.out.println("Iteration " + (k + 1) + ": min\t"
//					+ MVO.min(x0) + "\tmax\t" + MVO.max(x0));
		}
		if(k == maxIteration) {
			System.out.println("Iteration Has Reached Maximum Time! ");
			//return -1;
		}
		//System.out.println("Iteration Time " + k);
		return k;
	}
}
